#include"odom_progress.h"

odom_progress::odom_progress()
{
    odometry_subscriber_ = node_handle_.subscribe("Odometry", 20, &odom_progress::OdomCallback,
                                                 this,  ros::TransportHints().tcpNoDelay());
    odom_publisher_ = node_handle_.advertise<nav_msgs::Odometry>("odom", 20, this);
    odom_init();
}

odom_progress::~odom_progress()
{
}

void odom_progress::OdomCallback(const nav_msgs::Odometry::ConstPtr &msg)
{
    odom_msg_.pose.pose.position.x = msg->pose.pose.position.x;
    odom_msg_.pose.pose.position.y = msg->pose.pose.position.y;
    odom_msg_.pose.pose.position.z = msg->pose.pose.position.z;

    odom_msg_.pose.pose.orientation.x = msg->pose.pose.orientation.x;
    odom_msg_.pose.pose.orientation.y = msg->pose.pose.orientation.y;
    odom_msg_.pose.pose.orientation.z = msg->pose.pose.orientation.z;
    odom_msg_.pose.pose.orientation.w = msg->pose.pose.orientation.w;

    odom_msg_.header.frame_id = msg->header.frame_id;
    odom_msg_.header.stamp = msg->header.stamp;
    odom_publisher_.publish(odom_msg_);

    odom_transform.setOrigin( tf::Vector3(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z));
    odom_q.setX(msg->pose.pose.orientation.x);
    odom_q.setY(msg->pose.pose.orientation.y);
    odom_q.setZ(msg->pose.pose.orientation.z);
    odom_q.setW(msg->pose.pose.orientation.w);
   
    odom_transform.setRotation(odom_q);

    odom_broadcaster.sendTransform(tf::StampedTransform(odom_transform, ros::Time::now(), "odom", "base_footprint"));
    ROS_INFO("The transform has broadcasted!");
}
void odom_progress::odom_init(void)
{
    odom_msg_.pose.pose.position.x = 0;
    odom_msg_.pose.pose.position.y = 0;
    odom_msg_.pose.pose.position.z = 0;

    odom_msg_.pose.pose.orientation.x = 0;
    odom_msg_.pose.pose.orientation.y = 0;
    odom_msg_.pose.pose.orientation.z = 0;
    odom_msg_.pose.pose.orientation.w = 0;

    odom_msg_.header.frame_id = "odom";
    
    odom_publisher_.publish(odom_msg_);

    odom_transform.setOrigin( tf::Vector3(0, 0, 0));
    odom_q.setX(0);
    odom_q.setY(0);
    odom_q.setZ(0);
    odom_q.setW(0);
   
    odom_transform.setRotation(odom_q);
    
    odom_broadcaster.sendTransform(tf::StampedTransform(odom_transform, ros::Time::now(), "odom", "base_link"));
    ROS_INFO("The transform has broadcasted!");
}

int main(int argc, char** argv){
   ros::init(argc, argv, "odom_manager");
   odom_progress odom_project;

   ros::MultiThreadedSpinner spinner(2); // Use 2 threads
    spinner.spin(); // spin() will not return until the node has been shutdown
    return 0;

}